//
// Created by geoffrey on 24-11-16.
//
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace Eigen;
#include "cross.h"
//求解反对称矩阵
Matrix<double, 3, 3> S(const Matrix<double, 3, 1>& w)
{
    Matrix<double, 3, 1> I;
    Matrix<double, 3, 3> w_hat;
    I << 1.0, 0.0, 0.0;
    Matrix<double, 3, 1> w_hat1 = w.cross(I);
    I << 0.0, 1.0, 0.0;
    Matrix<double, 3, 1> w_hat2 = w.cross(I);
    I << 0.0, 0.0, 1.0;
    Matrix<double, 3, 1> w_hat3 = w.cross(I);
    w_hat << w_hat1 , w_hat2 , w_hat3;
    return w_hat;

}